#include "U2Plane.h"

#include "U2Matrix3.h"
#include "U2AxisAlignedBox3.h" 


U2EG_NAMESPACE_USING


//-----------------------------------------------------------------------
U2Plane::U2Plane ()
{
	normal = U2Vector3::ZERO;
	d = 0.0;
}
//-----------------------------------------------------------------------
U2Plane::U2Plane (const U2Plane& rhs)
{
	normal = rhs.normal;
	d = rhs.d;
}
//-----------------------------------------------------------------------
U2Plane::U2Plane (const U2Vector3& rkNormal, u2real fConstant)
{
	normal = rkNormal;
	d = -fConstant;
}
//---------------------------------------------------------------------
U2Plane::U2Plane (u2real a, u2real b, u2real c, u2real _d)
	: normal(a, b, c), d(_d)
{
}
//-----------------------------------------------------------------------
U2Plane::U2Plane (const U2Vector3& rkNormal, const U2Vector3& rkPoint)
{
	redefine(rkNormal, rkPoint);
}
//-----------------------------------------------------------------------
U2Plane::U2Plane (const U2Vector3& rkPoint0, const U2Vector3& rkPoint1,
	const U2Vector3& rkPoint2)
{
	redefine(rkPoint0, rkPoint1, rkPoint2);
}
//-----------------------------------------------------------------------
u2real U2Plane::getDistance (const U2Vector3& rkPoint) const
{
	return normal.dotProduct(rkPoint) + d;
}
//-----------------------------------------------------------------------
U2Plane::Side U2Plane::getSide (const U2Vector3& rkPoint) const
{
	u2real fDistance = getDistance(rkPoint);

	if ( fDistance < 0.0 )
		return U2Plane::NEGATIVE_SIDE;

	if ( fDistance > 0.0 )
		return U2Plane::POSITIVE_SIDE;

	return U2Plane::NO_SIDE;
}


//-----------------------------------------------------------------------
U2Plane::Side U2Plane::getSide (const U2AxisAlignedBox3& box) const
{
	if (box.isNull()) 
		return NO_SIDE;
	if (box.isInfinite())
		return BOTH_SIDE;

    return getSide(box.getCenter(), box.getHalfSize());
}
//-----------------------------------------------------------------------
U2Plane::Side U2Plane::getSide (const U2Vector3& centre, const U2Vector3& halfSize) const
{
    // Calculate the distance between box centre and the plane
    u2real dist = getDistance(centre);

    // Calculate the maximise allows absolute distance for
    // the distance between box centre and plane
    u2real maxAbsDist = normal.absDotProduct(halfSize);

    if (dist < -maxAbsDist)
        return U2Plane::NEGATIVE_SIDE;

    if (dist > +maxAbsDist)
        return U2Plane::POSITIVE_SIDE;

    return U2Plane::BOTH_SIDE;
}
//-----------------------------------------------------------------------
void U2Plane::redefine(const U2Vector3& rkPoint0, const U2Vector3& rkPoint1,
	const U2Vector3& rkPoint2)
{
	U2Vector3 kEdge1 = rkPoint1 - rkPoint0;
	U2Vector3 kEdge2 = rkPoint2 - rkPoint0;
	normal = kEdge1.crossProduct(kEdge2);
	normal.normalise();
	d = -normal.dotProduct(rkPoint0);
}
//-----------------------------------------------------------------------
void U2Plane::redefine(const U2Vector3& rkNormal, const U2Vector3& rkPoint)
{
	normal = rkNormal;
	d = -rkNormal.dotProduct(rkPoint);
}
//-----------------------------------------------------------------------
U2Vector3 U2Plane::projectVector(const U2Vector3& p) const
{
	// We know plane normal is unit length, so use simple method
	U2Matrix3 xform;
	xform[0][0] = 1.0f - normal.x * normal.x;
	xform[0][1] = -normal.x * normal.y;
	xform[0][2] = -normal.x * normal.z;
	xform[1][0] = -normal.y * normal.x;
	xform[1][1] = 1.0f - normal.y * normal.y;
	xform[1][2] = -normal.y * normal.z;
	xform[2][0] = -normal.z * normal.x;
	xform[2][1] = -normal.z * normal.y;
	xform[2][2] = 1.0f - normal.z * normal.z;
	return xform * p;
}
//-----------------------------------------------------------------------
u2real U2Plane::normalise(void)
{
    u2real fLength = normal.length();

    // Will also work for zero-sized vectors, but will change nothing
	// We're not using epsilons because we don't need to.
    // Read http://www.ogre3d.org/forums/viewtopic.php?f=4&t=61259
    if ( fLength > u2real(0.0f) )
    {
        u2real fInvLength = 1.0f / fLength;
        normal *= fInvLength;
        d *= fInvLength;
    }

    return fLength;
}
//-----------------------------------------------------------------------
std::ostream& operator<< (std::ostream& o, const U2Plane& p)
{
	o << "U2Plane(normal=" << p.normal << ", d=" << p.d << ")";
	return o;
}
